/*
 * @Description: Robot chassis motor communication header file
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2025-08-16 09:48:17
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-08-21 13:49:55
 * Copyright (C) 2024-2050 Lens All rights reserved.
 */

#ifndef __CHASSIS_MOTOR_H__
#define __CHASSIS_MOTOR_H__

#include <arpa/inet.h>
#include <unistd.h>

#include <chrono>
#include <cstring>
#include <geometry_msgs/msg/twist.hpp>
#include <iostream>
#include <mutex>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <thread>
#include <vector>

using namespace std;

struct Pose2D {
  double x{0.0}, y{0.0}, yaw{0.0};
};

typedef struct {
  double x;
  double y;
  double theta;

  double vx;
  double v_theta;
} Odom;

#define AH (0.135)
#define HB (0.135)
#define FE (0.135)
#define EG (0.135)
#define AB (AH + HB)
#define FG (AB)
#define AC (0.640)
#define AF (AC / 2)
#define OP (AC / 2)

typedef struct {
  double _OAP_Left;
  double _OHP_Mid;
  double _OBP_Right;
  double OA_L;
  double OH_M;
  double OB_R;
} RoberDegree;

typedef struct {
  double dCulMin;
  double dCulMid;
  double dCulMax;
} RoberDegreeSort;

typedef struct {
  double left_front_speed;
  double left_rear_speed;
  double right_front_speed;
  double right_rear_speed;
} RobotWheelSpeed;

typedef struct {
  double left_front_angle;
  double left_rear_angle;
  double right_front_angle;
  double right_rear_angle;
} RobotWheelAngle;

class ChassisKinematics : public rclcpp::Node {
 public:
  ChassisKinematics();

  void timer();

 public:
  // Defines wheel status parameters
  double m_wheel_distance_;
  double m_wheel_diameter_;

  int32_t m_left_wheel_circles_;  // unit: 0.001 circle
  int32_t m_left_wheel_speed_;
  int16_t m_left_wheel_current_;
  int8_t m_left_wheel_temperature_;
  int8_t m_left_wheel_status_;
  int8_t m_left_wheel_error_num_;

  int32_t m_right_wheel_circles_;  // unit: 0.001 circle
  int32_t m_right_wheel_speed_;
  int16_t m_right_wheel_current_;
  int8_t m_right_wheel_temperature_;
  int8_t m_right_wheel_status_;
  int8_t m_right_wheel_error_num_;

  // Defines odom parameters
  Odom m_current_odom_;
  int32_t m_last_left_wheel_circles_;
  int32_t m_last_right_wheel_circles_;

  // Defines ultrasonic distance
  double m_ultrasonic_distance_;  // unit: m
  std::vector<double> m_distance_samples_;

 public:
  // Controls wheels speed by id
  void motorSpeedControl(int id, double speed);

  // Controls whole chassis by linear velocity and angular velocity
  void chassisVelocityControl(double linear_velocity, double angular_velocity);

  // Sets kinematics data for chassis
  void setKinematicsParameters(const double &wheel_distance);

  // Calculates every wheel speed by chassis move speed
  void kinematicForward(const double &linear_velocity,
                        const double &angular_velocity,
                        double &left_wheel_velocity,
                        double &right_wheel_velocity);

  // Calculates chassis moving linear velocity and angular velocity by wheel
  // velocity
  void kinematicInverse(const double &left_wheel_velocity,
                        const double &right_wheel_velocity,
                        double &linear_velocity, double &angular_velocity);

  // Processes chassis motor status response
  void processChassisMotorStatus(const unsigned char *buffer,
                                 int receive_msg_len);

  // Calcualtes chassis odom by motor status
  Odom updateOdom(double v_l, double v_r, double dt, double wheel_distance);

  /************************************************************************


                r	X	  		s	  Y t	    Z



P			A****************H****************B



O			F****************E****************G



K			C****************J****************D

        AH=HB=FE=EG=CJ=JD=239mm
        AF=235mm,AC=470mm,AB=478mm,<rao=<sho=<tbo=90Degree

        <rax=<OAP,<shy=<ohp,<tbz=<obp

*************************************************************************/
  RoberDegreeSort minDegreeToOther(double min_degree);
  RoberDegreeSort maxDegreeToOther(double max_degree);
  RoberDegree degreeRightToOther(double dRight);
  RoberDegree degreeLeftToOther(double dLeft);

  /************************************************************************


                r	X	  		s	  Y t	    Z



P			A****************H****************B



O			F****************E****************G



K			C****************J****************D

        AH=HB=FE=EG=CJ=JD=239mm
        AF=235mm,AC=470mm,AB=478mm,<rao=<sho=<tbo=90Degree

        <rax=<OAP,<shy=<ohp,<tbz=<obp

*************************************************************************/
  RoberDegree degreeMiddleToOther(double middle_angle);
  void doubleAckermannIK(double linear_speed, double turn_speed);
  void doubleAckermannFK(const RobotWheelSpeed &wheel_speed,
                         const RobotWheelAngle &wheel_angle,
                         geometry_msgs::msg::Twist &robot_speed);

 private:
  double PB;
  double PH;
  double PA;
  RobotWheelSpeed wheel_speed;
  Pose2D pose_;
};

#endif